/**
  ******************************************************************************
  * @file    Devices_Lidar.c
  * @author  AnshininTakaha
  * @version V1.3
  * @date		 2021/12/27
  * @brief   The file of Pacecat lidar functions.
	* @copyright (c) 2017-2021 JZI. All rights reserved.
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "Devices_Lidar.h"

#include "Devices_E22.h"
#include "Devices_Battary.h"
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* INSPECT */
int Lidar_FrameCheck(uint8_t *LidarMsg);
void Lidar_PusherDataSumCheck(Lidar_MessagePusher_t* Pusher);

/* Private functions ---------------------------------------------------------*/
/**
 * @brief  [INSPECT] 						Check Lidar SOF CRC PCN Functions.
 * @param  uint8_t *LidarMsg: 	A Set of Lidar Original Message Relay for Sumcheck.
 * @return uint16_t 						PointCloud Number.
 */
int Lidar_FrameCheck(uint8_t *LidarMsg)
{
	int SumValue = 0;
	int PointClouds_Number = 0;
	
	/* Check SOF CRC PCN */
	if(LidarMsg[0] == 0xCE && LidarMsg[1] == 0xFA)
	{
		/* Get Point Cloud Number */
		PointClouds_Number = LidarMsg[2] | LidarMsg[3] << 8;
		
		/* Get Point Cloud SumCheck */
		for(int i=2;i<((2*PointClouds_Number)+6);i+=2)
		{
			SumValue += LidarMsg[i] | LidarMsg[i+1] << 8;
		}
		
		if((LidarMsg[2*PointClouds_Number+8-1] << 8 | \
			  LidarMsg[2*PointClouds_Number+8-2]) == \
		    (SumValue & 0xFFFF))
		{
			return PointClouds_Number;
		}
		else
		{
			return 0;
		}
	}
	else
	{
		return 0;
	}
}



/**
 * @brief [ALGORITHM] 										Lidar Pusher Data SumCheck.
 * @param Lidar_MessagePusher_t* Pusher: 	Pusher sum check.
 * @return void.
 */
void Lidar_PusherDataSumCheck(Lidar_MessagePusher_t* Pusher)
{
	Pusher->SumCheck = (Pusher->LidarMode + 
											Pusher->LidarDataState +
											Pusher->Min_LineDistance + 
											Pusher->Min_LineAngle +
											Pusher->Tree_Barrier1_X +
											Pusher->Tree_Barrier1_Y +
											Pusher->Tree_Barrier2_X +
											Pusher->Tree_Barrier2_Y +
											Pusher->Battary_Voltage) & 0xFFFF;
}


/* Exported mention ----------------------------------------------------------*/
_RETURN_STATE Lidar_USART_IT_Register(UART_HandleTypeDef* huartx);
_RETURN_STATE Lidar_PWR_Init(void);

Lidar_DataTable_t Lidar_Process(uint8_t *LidarMsg);

Lidar_Calculate_t Lidar_Algorithm_DataTableCalculate(Lidar_DataTable_t Lidar_DataTable);
void Lidar_Algorithm_DataTableMerge(Lidar_DataTable_t Lidar_MsgPackage, \
																	  Lidar_Calculate_t AC_In, \
																	  Lidar_Calculate_t* AC_Merge);
void Lidar_Algorithm_MergeAxisCalculate(Lidar_Calculate_t* AC_Merge);
void Lidar_PushMessageOriganize(Lidar_Calculate_t ALG_CM, \
																Lidar_MessagePusher_t* Pusher);

void Lidar_PushLidarDatatoReceiver(Lidar_MessagePusher_t Pusher);

uint8_t Add_Lidar_DataTableList(Lidar_DataTable_t *lt);
/* Exported variables --------------------------------------------------------*/
/* Lidar Function */
Lidar_Fuc Lidar_Function = Lidar_FucGroundInit;
#undef Lidar_FucGroundInit

/* Lidar Buffer */
/* 	50C-3 Lidar: 
		Max Point:200 
		Total Frame:410 */
uint8_t Lidar_Buffer[Lidar_BufferLength];

/* Algorithm Line Spacing */
uint16_t Algorithm_LineSpacing_M;

/* Exported functions --------------------------------------------------------*/
/**
 * @brief 	[REGISTER_INIT] 							The register init of Lidar module.
 * @param  	UART_HandleTypeDef* huartx: 	The target of the hooking usart.
 * @return 	void.
 */
_RETURN_STATE Lidar_USART_IT_Register(UART_HandleTypeDef* huartx)
{
	/* Check the register UART handle allocation */
	if(huartx == NULL)
	{
		return RETURN_ERROR_PARAM;
	}
	
	/* Clear the Flag First for Prevent IDLE Flag Broken Init */
   __HAL_UART_CLEAR_IDLEFLAG(huartx);
	
	/* Open the Serial port Using Polling */
  __HAL_UART_ENABLE(huartx);
	
	/* Receive IT Enable: DMARx */
	__HAL_UART_ENABLE_IT(huartx,UART_IT_IDLE);
	
	/* Activation the first Receive */
	HAL_UART_Receive_DMA(huartx, \
											Lidar_Buffer, \
											Lidar_BufferLength);
	
	/* Transmit IT Enable: DMATx */
  //None..
	//Mode Normal..
	
	/* USART Register */
	Lidar_Function.Lidar_Register = huartx;
	
	return RETURN_OK;
}


/**
 * @brief  [REGISTER_INIT] The lidar power IO init & reInit functions.
 * @param  void.
 * @return void.
 */
_RETURN_STATE Lidar_PWR_Init(void)
{
	if(Lidar_USART2_PWR_Get() != GPIO_PIN_SET)
	{
		Lidar_USART2_PWR_SetON();
	}
	else
	{
		Lidar_USART2_PWR_SetOFF();
		HAL_Delay(50);
		Lidar_USART2_PWR_SetON();
	}
	
	return RETURN_OK;
}



/**
 * @brief The lidar data depkg process function.
 * @param uint8_t *LidarMsg: A Set of Lidar Original Message.
 *
 * @attention The Lidar package of message begins from 180 ,End at 3420.
 *						The Max points of the Lidar package set is relay to the lidar
 *						which using.
 *            More info please check the maunal & buffer init statement.
 *            PS: Lidar Set Begin Angle at:
 *						(50C-3):180 540 900 1260 1620 1980 2340 2700 3060 3420.
 *
 * @return Lidar_MsgPackage_t  A TableData of a set of the Lidar Data.
 */
Lidar_DataTable_t Lidar_Process(uint8_t *LidarMsg)
{
	/* Create store value */
	Lidar_DataTable_t Lidar_DataTable = {0};
	int PointClouds_Number = 0;
	
	/* Get cloud point number */
	PointClouds_Number = Lidar_FrameCheck(LidarMsg);
	if(PointClouds_Number)
	{
		/* Frame check past */
		/* Get PointClouds_Number*/
		Lidar_DataTable.PointClouds_Number = PointClouds_Number;
		
		/* Lidar Angle uint 36 degree as 360*/
		float AnglePerData = 360.0f / Lidar_DataTable.PointClouds_Number;
		
		/* Get Begin Angle*/
		Lidar_DataTable.BeginAngle = LidarMsg[4] | LidarMsg[5] << 8;
		
		/* Angle Bias for 0 Degree face in front of the Logo */
		/* FrontSide		 (Right)	    (Left)	    (Original)
			 PACECAT Logo: 90 deg      	270 deg	    0/360 deg 
			 QR Code:      180 deg     	360/0 deg		90    deg
			 Interface     270 deg     	90 deg			180   deg
			 Blank         360/0 deg   	180 deg			270   deg */
		
		/* Front change */
		#ifdef USE_RIGHT_FRONT
		Lidar_DataTable.BeginAngle += 900.0f;//Right
		#endif
		#ifdef USE_LEFT_FRONT
		Lidar_DataTable.BeginAngle -= 900.0f;//Left
		#endif
		
		/* Zero crossing processing */
		if(Lidar_DataTable.BeginAngle >= 3600.0f)
		{
			Lidar_DataTable.BeginAngle -= 3600.0f;
		}
		else if(Lidar_DataTable.BeginAngle < 0.0f)
		{
			Lidar_DataTable.BeginAngle += 3600.0f;
		}
		else;
		
		/* Table Data Create */
		for(int i=0;i<Lidar_DataTable.PointClouds_Number;i++)
		{
			/* 50C-3Angle Unit: Degree*10 */
			Lidar_DataTable.AngleTable[i] = Lidar_DataTable.BeginAngle + \
			(i * AnglePerData);
			if(Lidar_DataTable.AngleTable[i] >= 3600.0f)
			{
				Lidar_DataTable.AngleTable[i] -= 3600.0f;
			}
			else if(Lidar_DataTable.AngleTable[i] < 0.0f)
			{
				Lidar_DataTable.AngleTable[i] += 3600.0f;
			}
			else;
			
			#ifdef USE_LEFT_FRONT
			/* Opposite the anglar table from cw to ccw */
			/* Adaptation to the left side fronting */
			Lidar_DataTable.AngleTable[i] = \
			3600.0f - Lidar_DataTable.AngleTable[i];
			#endif
			
			
			/* 50C-3Distance Unit: MM */
			Lidar_DataTable.DistanceTable[i] = (LidarMsg[6+(2*i)] | \
			LidarMsg[7+(2*i)] << 8);
		}
		
		return Lidar_DataTable;
	}
	else
	{
		/* Frame check Unpast */
		return Lidar_DataTable;
	}
}

#define C_RADAR_ANGLE2RAD_F (0.00174532925f)
static uint32_t lidar_point_cnt = 0,runcnt = 0;
uint32_t n1 = 0, n2 = 0, n3 = 0, n4 = 0;
float dis1 = 0.0f, dis2 = 0.0f, dis3 = 0.0f, dis4 = 0.0f;
float ddis1 = 0.0f, ddis2 = 0.0f, ddis3 = 0.0f, ddis4 = 0.0f;
uint8_t Add_Lidar_DataTableList(Lidar_DataTable_t *lt)
{
    int k = 0;
    for(uint16_t i=0;i<lt->PointClouds_Number;i++){
        if (lt->AngleTable[i] < 20 || lt->AngleTable[i] > 3580) 
        {
            n1++;
            dis1 += (float)lt->DistanceTable[i];
        }
        else if (lt->AngleTable[i] > 880 && lt->AngleTable[i] < 920)
        {
            n2++;
            dis2 += (float)lt->DistanceTable[i];
        }
        else if (lt->AngleTable[i] > 1780 && lt->AngleTable[i] < 1820)
        {
            n3++;
            dis3 += (float)lt->DistanceTable[i];
        }
        else if (lt->AngleTable[i] > 2680 && lt->AngleTable[i] < 2720)
        {
            n4++;
            dis4 += (float)lt->DistanceTable[i];
        }

        if(k > 1)
        {
            k = 0;
            lt->x_array[lidar_point_cnt] = (float)lt->DistanceTable[i] * arm_cos_f32(lt->AngleTable[i] * C_RADAR_ANGLE2RAD_F);
            lt->y_array[lidar_point_cnt] = (float)lt->DistanceTable[i] * arm_sin_f32(lt->AngleTable[i] * C_RADAR_ANGLE2RAD_F);
            lidar_point_cnt++;
            if(lidar_point_cnt>=TOTAL_NUM){
                lidar_point_cnt = 0;
            }
        }
        k++;
    }
	runcnt++;
	if(runcnt>=10){
		runcnt = 0;
        ddis1 = dis1 / (float)n1;
        ddis2 = dis2 / (float)n2;
        ddis3 = dis3 / (float)n3;
        ddis4 = dis4 / (float)n4;
        dis1 = 0.0f;
        dis2 = 0.0f;
        dis3 = 0.0f;
        dis4 = 0.0f;
        n1 = 0;
        n2 = 0;
        n3 = 0;
        n4 = 0;
		lt->TotalPointClouds_Number = lidar_point_cnt;
		lidar_point_cnt = 0;
		return 1;
	}
	else{
		return 0; 
	}
}


/**
 * @brief Lidar each DataTable Calculate function.
 * @param Lidar_MsgPackage  	The Origin depkg data from the point-cloud Lidar.
 *
 * @attention This Algorithm Only Calculate each DataTable from Lidar Original Data.
 *						Which means that each After-Calculate Algorithm_Calculate_t struct only
 *						include the vaild Data (Distance & Angle) of each DataTable,But NOT the
 *						Data of all (A Round).
 *
 *						The function Lidar_Algorithm_Merge will Merge all (A Round) Data after 
 *						this Calculations.(Reduce Algorithm Complexity & Prevents the data stack 
 *						from taking up too much space)
 *
 * @return Algorithm_Calculate_t  The After Calculate Data of each Table.
 */
Lidar_Calculate_t Lidar_Algorithm_DataTableCalculate(Lidar_DataTable_t Lidar_DataTable)
{
	/* Store the Minimun Distance & Angle & index */
	Lidar_Calculate_t Lidar_Calculate_Out = {0};
	
	/* Reset to default value */
	Lidar_Calculate_Out.Line_Distance = Default_Line_Distance_Max;
	Lidar_Calculate_Out.TreeBarrier_Distance = Default_Tree_Distance_Max;
	
	/* Store the Value include 25 degree of Tree Barrier */
	uint16_t TreeBarrier_Distance_Inc250 = Default_Tree_Distance_Max;
	float TreeBarrier_Angle_Inc250 = 0.0f;
	
	
	/* Process the Data */
	for(int index=0;index < Lidar_DataTable.PointClouds_Number;index++)
	{
		/* =====Check Barrier Line in front of the Plane===== */
		/* Bias Angle: +5 ~ -25 */
		/* Bias Distance: 500MM ~ 6000MM */
		if(Lidar_DataTable.AngleTable[index] > Default_Line_Angle_Min || \
			 Lidar_DataTable.AngleTable[index] < Default_Line_Angle_Max)
		{
			if(Lidar_DataTable.DistanceTable[index] > Default_Line_Distance_Min && \
				 Lidar_DataTable.DistanceTable[index] < Default_Line_Distance_Max)
			{
				/* Find Minimun Distance & Angle & Index */
				/* If Equal Store the Original Data */
				if(Lidar_DataTable.DistanceTable[index] < \
					 Lidar_Calculate_Out.Line_Distance)
				{
					Lidar_Calculate_Out.Line_Distance = \
					Lidar_DataTable.DistanceTable[index];
					
					Lidar_Calculate_Out.Line_Angle = \
					Lidar_DataTable.AngleTable[index];
				}
			}
		}
		
		/* =====Check Tree Barriers Under the Plane===== */
		/* Bias Angle: -0 ~ -180 */
		/* Bias Distance: 500MM ~ 30000MM */
		/* Area Cutting for the 25 Line Judge Area */
		if(Lidar_DataTable.AngleTable[index] > Default_Tree_Angle_Min && \
			 Lidar_DataTable.AngleTable[index] < Default_Tree_Angle_Max)
		{
			if(Lidar_DataTable.DistanceTable[index] > Default_Tree_Distance_Min &&
				 Lidar_DataTable.DistanceTable[index] < Default_Tree_Distance_Max)
			{
				/* The Tree Barrier is including in the Line Barrier Check Area */
				if(Lidar_DataTable.AngleTable[index] <= 250)
				{
					if(Lidar_DataTable.DistanceTable[index] < TreeBarrier_Distance_Inc250)
					{
						/* Find Minimun Distance & Angle */
						/* If Equal Store the Original Data */
						TreeBarrier_Distance_Inc250 = Lidar_DataTable.DistanceTable[index];
						TreeBarrier_Angle_Inc250 = Lidar_DataTable.AngleTable[index];
					}
				}
				/* The Tree Barrier is no including in the Line Barrier Check Area */
				else if(Lidar_DataTable.AngleTable[index] > 250)
				{
					if(Lidar_DataTable.DistanceTable[index] <= \
						Lidar_Calculate_Out.TreeBarrier_Distance)
					{
						/* Find Minimun Distance & Angle */
						/* If Equal Store the Original Data */
						Lidar_Calculate_Out.TreeBarrier_Distance = \
						Lidar_DataTable.DistanceTable[index];
						
						Lidar_Calculate_Out.TreeBarrier_Angle = \
						Lidar_DataTable.AngleTable[index];
					}
				}
				else{/* Pretent Logic illegal & Reduce Algorithm Complexity*/}
			}
		}
	}
	
	/* Plane is [Higher] than the Line Barrier */
	/* Judge Angle 0 ~ 25 Degree */
	if(Lidar_Calculate_Out.Line_Angle <= 250 && \
		Lidar_Calculate_Out.Line_Angle > 0)
	{
		/* TreeBarrier_Distance_Inc250 & TreeBarrier_Angle_Inc250 Data Can not be Use */
		/* TreeBarrier Data Maybe illegal */
	}
	else /* Plane is [Equal] or [Lower] than the Line Barrier */
	{
		if(Lidar_Calculate_Out.TreeBarrier_Distance < \
			TreeBarrier_Distance_Inc250)
		{/* Move Minimun Value into Algorithm_Calculate_Out */}
		else
		{/* Move Minimun Value into Algorithm_Calculate_Out */
			Lidar_Calculate_Out.TreeBarrier_Distance = \
			TreeBarrier_Distance_Inc250;
			Lidar_Calculate_Out.TreeBarrier_Angle = \
			TreeBarrier_Angle_Inc250;
		}
	}
	return Lidar_Calculate_Out;
}






/**
 * @brief Merge All the Lidar DataTables Calculate function.
 * @param Lidar_MsgPackage  	The Origin depkg data from the point-cloud Lidar.
 * @param AC_In  							Each After-Calculate DataTable from Lidar.
 * @param AC_Merge  					The Merge Table from all the DataTable,Propose the vaild Value.
 *
 * @attention This Algorithm Merge All the DataTables from function Lidar_Algorithm_TableCalculate.
 *						In order to get the vaild value per round, and Send to the Queue to make sure it is in order.
 *
 * @return void.
 */
void Lidar_Algorithm_DataTableMerge(Lidar_DataTable_t Lidar_MsgPackage, \
																	  Lidar_Calculate_t AC_In, \
																	  Lidar_Calculate_t* AC_Merge)
{
	/* Get A New Round is Begin */
	if(Lidar_MsgPackage.BeginAngle == 180.0f)
	{
		/* Get the initial Original value */
		memcpy(AC_Merge,&AC_In,sizeof(Lidar_Calculate_t));
	}
	else/* Inside A Round Or Get A Round is End */
	{
		/* Minimun Line Barrier Distance from the new pkg is vaild */
		if(AC_In.Line_Distance < AC_Merge->Line_Distance)
		{
			AC_Merge->Line_Distance = AC_In.Line_Distance;
			AC_Merge->Line_Angle = AC_In.Line_Angle;
		}
		/* Minimun Tree Barrier Distance from the new pkg is vaild */
		if(AC_In.TreeBarrier_Distance < AC_Merge->TreeBarrier_Distance)
		{
			AC_Merge->TreeBarrier_Distance = AC_In.TreeBarrier_Distance;
			AC_Merge->TreeBarrier_Angle = AC_In.TreeBarrier_Angle;
		}
	}
}






/**
 * @brief Calculate the MergeTable X & Y Axis.
 * @param AC_Merge  	The Merge Table from all the DataTable,Propose the vaild Value.
 *
 * @attention This Algorithm Calculate the MergeTable X & Y Axis.
 *						In order to get the vaild value per round, and Send to the Queue to make sure it is in order.
 *						(Reduce Algorithm Complexity & Prevents the data stack from taking up too much space)
 *
 * @return void.
 */
void Lidar_Algorithm_MergeAxisCalculate(Lidar_Calculate_t* AC_Merge)
{
	/* Create Angle to Rad for the Calculate */
	float LineBarrier_AngletoRad = 0;
	float TreeBarrier_AngletoRad = 0;
	
	/* Create PI Value */
	//float PI = acos(-1);
	/* Using PI Value in arm_math.h */
	
	/* ========== Line Barrier Calculate Modify Picture ========== */
	/*  
	 *					-----
	 *		------	5		(Upon)
	 *	[P] -=-=-=-=-=-=-=-=-=-=-= Line (0 Degree)
	 *		--			25  (Under)
	 *			--
	 *				--
	 *					--
	 *            --
	*/
	
	/* Calculate the Line Barrier Axis X & Y Bias (When Line Barrier Exist) */
	if(AC_Merge->Line_Distance != Default_Line_Distance_Max)
	{
		/* Calculate the Line Barrier Angle to Rad */
		if(AC_Merge->Line_Angle > 1800.0f)
		{
			/* Angle Upon the 0 degree */
			LineBarrier_AngletoRad = \
			((3600.0f - AC_Merge->Line_Angle) / 10.0f) * (PI/180.0f);
		}
		else
		{
			/* Angle Under the 0 degree */
			LineBarrier_AngletoRad = \
			((0.0f - AC_Merge->Line_Angle) / 10.0f) * (PI/180.0f);
		}
		
		AC_Merge->Line_Distance_X = \
		AC_Merge->Line_Distance * arm_cos_f32(LineBarrier_AngletoRad);
		AC_Merge->Line_Distance_Y = \
		AC_Merge->Line_Distance * arm_sin_f32(LineBarrier_AngletoRad);
	}
	else
	{
		/* Refreash Angle & Distance */
		AC_Merge->Line_Angle = 0;
		AC_Merge->Line_Distance = 0;
		
		AC_Merge->Line_Distance_X = 0;
		AC_Merge->Line_Distance_Y = 0;
	}
	
	
	/* ========== Tree Barrier Calculate ========== */
	/* Tree Barrier Calculate Store Create */
	float TreeBarrier_X = 0;
	float TreeBarrier_Y = 0;
	
	/* Calculate which Line Side Closer to the TreeBarrier(When Line TreeBarrier Exist) */
	if(AC_Merge->TreeBarrier_Distance != Default_Tree_Distance_Max)
	{
		/* Tree Barrier Axis X & Y Calculate */
		TreeBarrier_AngletoRad = \
		((0.0f - AC_Merge->TreeBarrier_Angle) / 10.0f) * (PI/180.0f);
		TreeBarrier_X = AC_Merge->TreeBarrier_Distance \
		* cos(TreeBarrier_AngletoRad);
		TreeBarrier_Y = AC_Merge->TreeBarrier_Distance \
		* sin(TreeBarrier_AngletoRad);
		
		/* X&Y Axis calculate */
		if(TreeBarrier_X <= (AC_Merge->Line_Distance_X + \
			((Algorithm_LineSpacing_M) / 2.0f)))
		{
			/* X */
			AC_Merge->TreeBarrier_RelayLine1_X = \
			TreeBarrier_X - AC_Merge->Line_Distance_X;
				
			/* Y */
			AC_Merge->TreeBarrier_RelayLine1_Y = \
			-TreeBarrier_Y - AC_Merge->Line_Distance_Y;
		}
		else if(TreeBarrier_X > (AC_Merge->Line_Distance_X + \
				((Algorithm_LineSpacing_M) / 2.0f)))
		{
			/* X */
			AC_Merge->TreeBarrier_RelayLine2_X = \
			TreeBarrier_X - AC_Merge->Line_Distance_X - \
			(Algorithm_LineSpacing_M);
				
			/* Y */
			AC_Merge->TreeBarrier_RelayLine2_Y = \
			-TreeBarrier_Y - AC_Merge->Line_Distance_Y;
		}
		else{/* Pretent Logic illegal & Reduce Algorithm Complexity*/}
	}
	else
	{
		/* Refreash Angle & Distance */
		AC_Merge->TreeBarrier_Angle = 0;
		AC_Merge->TreeBarrier_Distance = 0;
		
		AC_Merge->TreeBarrier_RelayLine1_X = 0;
		AC_Merge->TreeBarrier_RelayLine1_Y = 0;
		AC_Merge->TreeBarrier_RelayLine2_X = 0;
		AC_Merge->TreeBarrier_RelayLine2_Y = 0;
	}
}







/**
 * @brief Origanize the Pre-push message.
 * @param AC_Merge The Merge Table from all the DataTable,Propose the vaild Value.
 * @param Pusher   The Message will be push
 * @return void.
 */
void Lidar_PushMessageOriganize(Lidar_Calculate_t ALG_CM, \
																Lidar_MessagePusher_t* Pusher)
{
	/* Store Receiver Data from Merge Algorithm Address */
	Pusher->SOF = 0xCDF1;
	Pusher->DataLength = 0x0009;
	
	/* If Detected Line */
	if(Pusher->Min_LineDistance != 0)
	{
		Pusher->LidarMode = Inspection;
	}
	else
	{
		Pusher->LidarMode = Idle;
	}
	
	/* Lidar State Data Checking Correctly */
	Pusher->LidarDataState = State_Correct;
	
	/* Line Barrier Merge Data Pass */
	/* Send Out Uint: Min_LineDistance: MM
										Min_LineAngle: Degree*10 */
	Pusher->Min_LineDistance = ALG_CM.Line_Distance;
	Pusher->Min_LineAngle = ALG_CM.Line_Angle;
	
	/* Tree Barrier Merge Data Pass */
	Pusher->Tree_Barrier1_X = ALG_CM.TreeBarrier_RelayLine1_X;
	Pusher->Tree_Barrier1_Y = ALG_CM.TreeBarrier_RelayLine1_Y;
	Pusher->Tree_Barrier2_X = ALG_CM.TreeBarrier_RelayLine2_X;
	Pusher->Tree_Barrier2_Y = ALG_CM.TreeBarrier_RelayLine2_Y;
	
	/* Battary Voltage Setting */
	Pusher->Battary_Voltage = LID_BAT_Remain_Voltage_to_Zero;
	
	/* Get SumCheck */
	Lidar_PusherDataSumCheck(Pusher);
	
}



/**
 * @brief Push the lidar data to the Receiver.
 * @param ALG_CM  	The Alogorithm After-Calculate Structure
 * @param Receiver  The Transfer Data Structure of Receiver(s)
 * @return void.
 */
void Lidar_PushLidarDatatoReceiver(Lidar_MessagePusher_t Pusher)
{
	
	HAL_UART_Transmit_DMA(E22_Function.E22_Register, \
												(uint8_t*)&Pusher.Lidar_MsgPusher, \
												Lidar_MsgPusherLength);
	
	HAL_Delay(50);

}

/* End -----------------------------------------------------------------------*/
